#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <iostream>
#include <fstream>
#include <string>
#include <pushmsg.h>
using namespace std;
//gps topic /gps

int main(int argc, char **argv)
{
    // Initialize ROS node
    ros::init(argc, argv, "publish_imu");
    ros::NodeHandle nh;
    // 创建发布节点
    ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu_data", 1000);
    //ros::Publisher gps_pub = nh.advertise<sensor_msgs::NavSatFix>("fix", 1000);
    // 从数据集中读取数据
    //string rowList[8] = {"34","35","37","38","39","43","45","47"};
    ros::Rate loop_rate(400); //发布频率
        string imu_file_path = "/home/gxx/imugps-ekf/kaist_urban08/urban08_data/xsens_imu.csv";
        ifstream imu_file(imu_file_path);
        if (!imu_file.is_open()) {
            ROS_ERROR_STREAM("Failed to open IMU data file: " << imu_file_path);
            exit(-1);
        }
        // Parse IMU data and publish as ROS messages
        string dataline;
        while (getline(imu_file, dataline)) {
            if(dataline.empty()){
                break;
            }
            pushimu(dataline, imu_pub);
            //pushgps(dataline, time, gps_pub);
            loop_rate.sleep();
        }
    while (ros::ok())
    {
        loop_rate.sleep();
    }
    return 0;
}
